#include "AddWholeBodyDynamicHandler.h"
#include "AddWholeBodyDynamicHandlerDialog.h"
#include <QFileDialog>
#include <QMessageBox>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include <MMM/Motion/Plugin/WholeBodyDynamicPlugin/WholeBodyDynamicSensor.h>

using namespace MMM;

AddWholeBodyDynamicHandler::AddWholeBodyDynamicHandler(QWidget* widget) :
    MotionHandler(MotionHandlerType::ADD_SENSOR, "Add whole body dynamic"),
    searchPath(std::string(MMMTools_SRC_DIR)),
    widget(widget)
{
}

void AddWholeBodyDynamicHandler::handleMotion(MotionList motions) {
    if (motions.size() > 0) {
        MMM::MotionList calculatableMotions;
        for (MMM::MotionPtr motion : motions) {
            if (!motion->getSensorByType(KinematicSensor::TYPE)) {
                MMM_INFO << "Ignoring motion '" + motion->getName() + "'! Because the motion doesn't contain a kinematic." << std::endl;
                continue;
            } else if (!motion->getSensorByType(ModelPoseSensor::TYPE)) {
                MMM_INFO << "Ignoring motion '" + motion->getName() + "'! Because the motion doesn't contain a model pose." << std::endl;
                continue;
            }
            else if (motion->getSensorByType(WholeBodyDynamicSensor::TYPE)) {
                MMM_INFO << "Ignoring motion '" + motion->getName() + "'! Because the motion already contains the whole body dynamic." << std::endl;
                continue;
            }

            calculatableMotions.push_back(motion);
        }
        if (calculatableMotions.size() == 0) {
            QMessageBox* msgBox = new QMessageBox(widget);
            msgBox->setText("No calculatable motions found!");
            msgBox->exec();
        } else {
            AddWholeBodyDynamicHandlerDialog* dialog = new AddWholeBodyDynamicHandlerDialog(widget, calculatableMotions);
            if (dialog->calculateWholeBodyDynamic()) emit openMotions(motions);
        }
    }
    else MMM_ERROR << "Cannot open add whole body dynamic sensor dialog, because no motions are present!" << std::endl;
}

std::string AddWholeBodyDynamicHandler::getName() {
    return NAME;
}
